using Game;
using NaughtyAttributes;
using System.Collections.Generic;
using UnityEngine;
using Utils;

[RequireComponent(typeof(Rigidbody))]
public class ShipPhysics : MonoBehaviour {
	[Header("Physics")]
	public float   MaxThrust       = 100f;
	public Vector3 TurnTorque      = new Vector3(90f, 25f, 45f);
	public float   ForceMultiplier = 1000f;
	public float   MaxVelocity     = 100f;
	[Range(0f, 100f)]
	public float   SpeedLimitForce = 30f;

	[Header("Autopilot")]
	public bool  Enabled             = true; // False если мы используем режим прямого управления (с джоя например)
	public float AggressiveTurnAngle = 10f;
	public bool  UseRollInAutopilot  = true;
	public bool  CompensateInertia   = true;
	public bool  ThrustAutopilot     = false;
	public bool EnableCollisionAvoidance = false;
	public float CollisionAvoidanceDistance = 10f;
	[Range(0f, 0.2f)]
	public float Deadzone            = 0.01f;
	[Range(0f, 50f)]
	public float MaxInertiaCompensation = 20f;

	[Header("PID Controllers")]
	public PID   PitchPID            = null;
	public PID   RollPID             = null;
	public PID   YawPID              = null;
	public PID   ThrustPID           = null;

	[Header("Input")]
	[Range(-1f, 1f)] public float Pitch  = 0f;
	[Range(-1f, 1f)] public float Yaw    = 0f;
	[Range(-1f, 1f)] public float Roll   = 0f;
	[Range(-1f, 1f)] public float Thrust = 0f;

	[Range(0, 1f)]   public float ThrustLimit = 1f;
	public float SpeedLimit = 20f;

	public float JumpImpulse = 2f;
	public float JumpInterval = 0.5f;

	[Header("Movement Target")]
	public Vector3 MoveTarget;

	[Header("Other")]
	public LayerMask RaycastMask;

	public EventBus<float> OnJump = new EventBus<float>();

	Rigidbody _rb = null;
	Vector3   _lastFrameVelocity = Vector3.zero;
	Vector3   _autopilotTarget;
	float     _lastJumpTime;

	public float RollOverride { get; set; } = 0f;
	public float PitchOverride { get; set; } = 0f;

	public Vector3 AngularVelocity {
		get {
			return _rb.angularVelocity;
		}
	}

	public float Velocity {
		get {
			return _rb.velocity.magnitude * Mathf.Sign(Vector3.Dot(transform.forward, _rb.velocity.normalized));
		}
	}

	public float TargetVelocity {
		get {
			return MaxVelocity * Thrust;
		}
	}

	[Button]
	public void TestJump() {
		Jump(-Vector3.right);
	}

	public void Jump(Vector3 direction) {
		if ( !GameManager.Instance.IsServer ) {
			return;
		}
		if ( Time.time - _lastJumpTime < JumpInterval ) {
			return;
		}
		_lastJumpTime = Time.time;
		direction = Vector3.ClampMagnitude(direction, 1f);
		_rb.AddRelativeForce(direction * JumpImpulse, ForceMode.Impulse);
		OnJump.Fire(direction.magnitude);
	}

	public Vector3 Acceleration { get {
			return (_rb.velocity - _lastFrameVelocity) / Time.fixedDeltaTime;
		}
	}

	void Awake() {
		_rb = GetComponent<Rigidbody>();
	}	

	private void OnDrawGizmos() {
		Gizmos.DrawWireSphere(_autopilotTarget, 1f);
	}

	private void RunAutopilot(Vector3 flyTarget, out float yaw, out float pitch, out float roll, out float thrust) {
		var avoidanceActive = false;
		if ( EnableCollisionAvoidance) {
			if ( CheckObstacles(out var avoidanceTarget) ) {
				flyTarget = avoidanceTarget;
				avoidanceActive = true;
			}
		}
		_autopilotTarget = flyTarget;
		var localFlyTarget = transform.InverseTransformPoint(flyTarget).normalized;
		var angleOffTarget = Vector3.Angle(transform.forward, flyTarget - transform.position);
		var dist = Vector3.Distance(transform.position, flyTarget);

		yaw = YawPID.Update(0, -localFlyTarget.x, Time.fixedDeltaTime);
		pitch = PitchPID.Update(0, localFlyTarget.y, Time.fixedDeltaTime);

		var rollBlend = Mathf.Clamp01(Mathf.InverseLerp(0f, AggressiveTurnAngle, angleOffTarget));
		if ( dist < 2f) {
			rollBlend = 0f;
		}

		var agressiveRoll = RollPID.Update(0, Mathf.Lerp(-transform.right.y, -localFlyTarget.x, rollBlend), Time.fixedDeltaTime);
		roll = agressiveRoll;
		pitch = Mathf.Clamp(pitch, -1f, 1f);
		yaw   = Mathf.Clamp(yaw, -1f, 1f);
		roll  = Mathf.Clamp(roll, -1f, 1f);

		if ( Mathf.Abs(pitch) < Deadzone ) {
			pitch = 0f;
		}
		if ( Mathf.Abs(yaw) < Deadzone ) {
			yaw = 0f;
		}
		if ( Mathf.Abs(roll) < Deadzone || !UseRollInAutopilot ) {
			roll = 0f;
		}

		var dirDot = Vector3.Dot(transform.TransformDirection(Vector3.forward), (flyTarget - transform.position).normalized);
	
		if ( dist < 1f ) {
			thrust = 0f;
		} else {
			thrust = Mathf.Clamp(ThrustPID.Update(dist * Mathf.Sign(dirDot), Time.fixedDeltaTime), -0.5f, avoidanceActive ? 0.7f : 1);
		}		
	}

	Vector3 _avoidancePoint;
	bool _activeAvoidance;

	bool CheckObstacles(out Vector3 avoidancePoint) {
		avoidancePoint = MoveTarget;
		CollisionAvoidanceDistance = Mathf.Clamp(Velocity* Velocity * 0.7f, 3f, 100);
		Debug.DrawRay(transform.position, transform.TransformDirection(Vector3.forward) * CollisionAvoidanceDistance, Color.cyan);

		var checkDistance = CollisionAvoidanceDistance;
		if ( _activeAvoidance ) {
			checkDistance = Mathf.Max(Vector3.Distance(_avoidancePoint, transform.position), checkDistance);
		}
		var distanceToTarget = Vector3.Distance(MoveTarget, transform.position);
		checkDistance = Mathf.Min(checkDistance, distanceToTarget);

		if ( !TestCastRaySphere(transform.position, (MoveTarget - transform.position).normalized, 0.6f, checkDistance, out var hit) ) {		
			if ( _activeAvoidance ) {
				//Debug.Log("avoidance reset");
			}
			_activeAvoidance = false;
			
			_avoidancePoint = MoveTarget;
			return false;
		}
		if ( Vector3.Distance(hit.point, MoveTarget) < 2f ) {
			return false; //хоть и препятствие, но оно слишком близко чтобы его облетать
		}
		Debug.DrawLine(transform.position, hit.point, Color.magenta);
		if ( _activeAvoidance ) {
			if ( CheckIfPointAccessible(_avoidancePoint) && Vector3.Distance(transform.position, _avoidancePoint) > 2f ) {
				avoidancePoint = _avoidancePoint;
				return true;
			}
			//Debug.Log("avoidance active, point inaccessible");
		}

		avoidancePoint = GetAvoidancePoint(CollisionAvoidanceDistance, hit.distance);
		_avoidancePoint = avoidancePoint;
		_activeAvoidance = true;
		return true;
	}

	bool TestCastRaySphere(Vector3 origin, Vector3 direction, float radius, float distance, out RaycastHit hit) {
		if ( Physics.Raycast(origin, direction, out hit, distance, RaycastMask, QueryTriggerInteraction.Ignore) ) {
			return true;
		}
		if ( Physics.SphereCast(origin, radius, direction, out hit, distance, RaycastMask, QueryTriggerInteraction.Ignore) ) {
			return true;
		}
		return false;
	}

	Vector3 GetAvoidancePoint(float maxDist, float obstacleDist) {
		var step = 10f;
		var distP = Mathf.Clamp( step * (1 -Mathf.InverseLerp(0f, 70f, obstacleDist)), 1f, 10f);

		_candidatePoints.Clear();
		CollectCandidatePointsForAxis(Vector3.up, distP, maxDist);
		CollectCandidatePointsForAxis(Vector3.right, distP, maxDist);
		if ( _candidatePoints.Count > 0 ) {
			var point = FindClosestToTargetCandidatePoint();
			return point.EndPoint;
		}

		//Debug.Log("Not found avoidance point");
		return MoveTarget;
	}

	void CollectCandidatePointsForAxis(Vector3 axis, float step, float maxDist) {
		var tryCount = 0;
		while ( tryCount < 10 ) {
			var sign = tryCount % 2 == 1 ? 1 : -1;
			var angle = (tryCount + 1) * step * sign;
			var vec = Quaternion.AngleAxis(angle, axis) * transform.TransformDirection(Vector3.forward);
			var point = transform.position + vec * (maxDist * 1.25f);
			if ( Vector3.Dot(vec, transform.TransformDirection(Vector3.forward)) < 0 ) {
				tryCount++;
				continue;
			}
			if ( CheckIfPointAccessible(point) ) {
				_candidatePoints.Add(new AvoidancePointData() { Direction = vec, EndPoint = point, TurnAngle = angle * sign });
				//return;
			}
			tryCount++;
		}
	}

	AvoidancePointData FindMinTurnCandidatePoint() {
		var minAngle = 360f;
		var minAngleIndex = 0;
		for ( int i = 0; i < _candidatePoints.Count; i++ ) {
			if ( Mathf.Abs(_candidatePoints[i].TurnAngle) < minAngle ) {
				minAngleIndex= i;
				minAngle = Mathf.Abs(_candidatePoints[i].TurnAngle);
			}
		}
		return _candidatePoints[minAngleIndex];
	}

	AvoidancePointData FindClosestToTargetCandidatePoint() {
		var minDist = float.MaxValue;
		var minDistIndex = 0;
		for ( int i = 0; i < _candidatePoints.Count; i++ ) {
			if ( Vector3.Distance(MoveTarget, _candidatePoints[i].EndPoint) < minDist ) {
				minDistIndex = i;
				minDist = Mathf.Abs(_candidatePoints[i].TurnAngle);
			}
		}
		return _candidatePoints[minDistIndex];
	}

	List<AvoidancePointData> _candidatePoints = new List<AvoidancePointData>(16);

	struct AvoidancePointData {
		public Vector3 EndPoint;
		public Vector3 Direction;
		public float TurnAngle;
	}

	bool CheckIfPointAccessible(Vector3 point) {
		return !TestCastRaySphere(transform.position, (point - transform.position).normalized, 0.6f, (point - transform.position).magnitude, out var hit);
	}

	private void FixedUpdate() {
		RunAutopilot(MoveTarget, out var autoYaw, out var autoPitch, out var autoRoll, out var autoThrust);

		Yaw   = autoYaw;
		Pitch = (Mathf.Abs(PitchOverride) > 0.1f) ? PitchOverride : autoPitch;
		Roll  = (Mathf.Abs(RollOverride)  > 0.1f) ? RollOverride  : autoRoll;

		if ( ThrustAutopilot ) {
			Thrust = autoThrust;
		}

		var v = _rb.velocity.magnitude;
		if (v < MaxVelocity * Mathf.Abs(Thrust) && v < SpeedLimit ) {
			var limitedThrust = Mathf.Clamp(Thrust, -ThrustLimit, ThrustLimit);
			_rb.AddRelativeForce(Vector3.forward * MaxThrust * limitedThrust * ForceMultiplier, ForceMode.Force);
		}

		_rb.AddRelativeTorque(new Vector3(TurnTorque.x * Pitch,
											TurnTorque.y * Yaw,
											-TurnTorque.z * Roll) * ForceMultiplier,
								ForceMode.Force);
		if ( CompensateInertia ) {
			RemoveExcessVelocity();
		}
		_lastFrameVelocity = _rb.velocity;
	}

	void RemoveExcessVelocity() {
		var velMag = _rb.velocity.magnitude;
		if ( velMag < 0.01f ) {
			return;
		}
		var proj = Vector3.Project(_rb.velocity, transform.forward * Mathf.Sign(Thrust));
		var comp = _rb.velocity - proj;
		var compForce = -Vector3.ClampMagnitude(comp, MaxInertiaCompensation);

		if ( Mathf.Abs(Velocity) > Mathf.Abs(TargetVelocity) ) {
			var brakeForce = -transform.forward * Mathf.Sign(Velocity) * ForceMultiplier * SpeedLimitForce;
			var energy = ((_rb.mass * (velMag * velMag)) * 0.5f) / Time.deltaTime;
			brakeForce = Vector3.ClampMagnitude(brakeForce, energy);
			_rb.AddForce(brakeForce);
		}

		Debug.DrawRay(transform.position, proj);
		Debug.DrawRay(transform.position, compForce, Color.red);
		_rb.AddForce(compForce * ForceMultiplier);
	}
}
